import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

def generate_launch_description():
    # 获取包路径
    slam_pkg = get_package_share_directory("slam_gmapping")
    slam_toolbox_pkg = get_package_share_directory("go2_agiros_slam")
    nav2_pkg = get_package_share_directory("mycar_navigation2")


    # 定义启动文件
    slam_launch = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
        os.path.join(slam_pkg, 'launch', 'slam_gmapping.launch.py')
    ),
    launch_arguments={
        'use_rviz': 'false'  # 强制覆盖
    }.items()
    )

    # slam_launch = IncludeLaunchDescription(
    # PythonLaunchDescriptionSource(
    #     os.path.join(slam_toolbox_pkg, 'launch', 'go2_slamtoolbox.launch.py')
    # ),
    # launch_arguments={
    #     'use_rviz': 'false'  # 强制覆盖
    # }.items()
    # )

    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(nav2_pkg, 'launch', 'nav2.launch.py')
        )
    )

    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        arguments=["-d", os.path.join(slam_toolbox_pkg, "rviz", "go2_auto.rviz")],
        output="screen",
    )

    go2_twist_node = Node(
        package="go2_twist_bridge_py",
        executable="twist_bridge",
        output="screen",
    )

    # 创建 LaunchDescription
    ld = LaunchDescription()

    # 先启动 SLAM（立即启动）
    ld.add_action(slam_launch)
    ld.add_action(rviz2_node)
    ld.add_action(go2_twist_node)
    
    # 延迟 10 秒后启动导航系统
    ld.add_action(
        TimerAction(
            period=10.0,  # 延迟时间（秒）
            actions=[nav2_launch]
        )
    )
    
    return ld